#include <algorithm>
#include <cmath>
#include <iostream>
#include <numeric>
#include <vector>
#include "elog/elog_interface.h"
#include "plan/plan_config.hpp"
#include "plan/plan_data_structure.hpp"
#include "plan/plan_impl/joint_planner.hpp"

namespace ZORAL
{
    int JointPlanFivePoly(const double &sample_rate, const JointPlannerProperties &planner)
    {
        ZLOGI("Joint Space Five-Poly Trajectory Planning...");

        if ((planner.time_lim <= 0 && planner.vel_lim <= 0))
        {
            ZLOGE("Joint Space Planning Failed, velocity and time limit invalid! time_lim: %f, vel_lim: %f",
                  planner.time_lim, planner.vel_lim);
            return E_PLAN_PARA_INVALID;
        }

        if (std::fabs(planner.pos_start - planner.pos_end) < PLAN_EPS)
        {
            ZLOGW("Joint Space Planning Failed, Start Point and End Point too closed! start: %f, end: %f",
                  planner.pos_start, planner.pos_end);
            if (planner.time_lim > 0)
            {
                int N = static_cast<int>(std::ceil(planner.time_lim * sample_rate));
                planner.traj.resize(N);
                std::fill(planner.traj.begin(), planner.traj.end(), planner.pos_start);
            }
            else
            {
                ZLOGE("time limit invalid: %f, planning failed", planner.time_lim);
            }
            return E_PLAN_POS_TOO_CLOSE;
        }

        double pos_dist = planner.pos_end - planner.pos_start;
        double time;
        if (planner.time_lim > 0)
        {
            //! 若设置了时间条件，则忽略设置的速度条件
            time = planner.time_lim;
            ZLOGD("Planner follow the time limit, whole trajectory will move within %f s.", time);
        }
        else
        {
            time = 15.0 / 8.0 * std::fabs(pos_dist) / planner.vel_lim;
            ZLOGD("Planner follow the velocity limit, max velocity: %f, whole trajectory will move within %f s.",
                  planner.vel_lim, time);
        }
        int N = static_cast<int>(std::ceil(time * sample_rate));
        planner.traj.resize(N);

        const double time_step = 1.0 / sample_rate;
        std::vector<double> a = {planner.pos_start,
                                 0.0,
                                 0.0,
                                 (10.0 * pos_dist) / std::pow(time, 3),
                                 (-15 * pos_dist) / std::pow(time, 4),
                                 (6 * pos_dist) / std::pow(time, 5)};
        double t;
        for (int i = 0; i < N; i++)
        {
            t = time_step * i;
            if (t < time)
            {
                planner.traj[i] = a[0] + a[1] * t + a[2] * std::pow(t, 2) + a[3] * std::pow(t, 3) +
                                  a[4] * std::pow(t, 4) + a[5] * std::pow(t, 5);
            }
            else
            {
                planner.traj[i] = planner.pos_end;
            }
        }

        return E_PLAN_NO_ERROR;
    }

//    int JointPlanCurveS(const double &sample_rate, JointPlan &planner)
//    {
//        ZLOGD("Joint Space S-Curve Planning...");
//        double J = planner.jerk_lim;
//        double a = planner.acc_lim;
//        double v = planner.vel_lim;
//
//        if ((J <= 0 || a <= 0 || v <= 0 || std::fabs(planner.pos_start - planner.pos_end) <= PLAN_EPS))
//        {
//            ZLOGE("Joint Space Planning Failed, position/velocity/acceleration/jerk invalid! q_start: %f, q_end: %f, "
//                  "vel_lim: %f, acc_lim: %f, jerk_lim: %f",
//                  planner.pos_start, planner.pos_end, v, a, J);
//            return E_PLAN_PARA_INVALID;
//        }
//        ZLOGD("Joint Space Planning Start, q_start: %f, q_end: %f, v: %f, a: %f, J: %f", planner.pos_start,
//              planner.pos_end, v, a, J);
//
//        double qs, qe;
//        if (planner.pos_end > planner.pos_start)
//        {
//            qs = planner.pos_start;
//            qe = planner.pos_end;
//        }
//        else
//        {
//            qs = planner.pos_end;
//            qe = planner.pos_start;
//        }
//        double q = qe - qs;
//
//        //! 定义系数矩阵和常数向量
//        double K[7][3] = {{J / 6, 0.0, 0.0},
//                          {a / 2, -a * a / (2 * J), 0.0},
//                          {-J / 6, a / 2 + (J * v) / (2 * a), -a * a / (2 * J) - (J * v * v) / (2 * a * a)},
//                          {v, 0.0, 0.0},
//                          {-J / 6, (J * q) / (2 * v), v - (J * q * q) / (2 * v * v)},
//                          {-a / 2, a * a / (2 * J) + (q * a) / v + v, 0.0},
//                          {J / 6, -a / 2 - (J * v) / (2 * a) - (J * q) / (2 * v),
//                           v + a * a / (2 * J) + (a * q) / v + (J * v * v) / (2 * a * a) + (J * q * q) / (2 * v * v) +
//                                   (J * q) / a}};
//        double C[7] = {qs,
//                       qs + a * a * a / (6 * J * J),
//                       qs + a * a * a / (6 * J * J) + (J * v * v * v) / (6 * a * a * a),
//                       -v * v / (2 * a) - (a * v) / (2 * J) + qs,
//                       qs - v * v / (2 * a) + (J * q * q * q) / (6 * v * v * v) - (a * v) / (2 * J),
//                       qs - a * a * a / (6 * J * J) - v * v / (2 * a) - (a * q * q) / (2 * v * v) - (a * v) / (2 * J) -
//                               (a * a * q) / (2 * J * v),
//                       qs - a * a * a / (6 * J * J) - v * v / (2 * a) - (J * v * v * v) / (6 * a * a * a) -
//                               (J * q * q * q) / (6 * v * v * v) - (a * q * q) / (2 * v * v) - (a * v) / (2 * J) -
//                               (J * q * q) / (2 * a * v) - (a * a * q) / (2 * J * v) - (J * q * v) / (2 * a * a)};
//
//        std::vector<double> t(7);
//        t[0] = a / J;
//        t[1] = v / a;
//        t[2] = t[0] + t[1];
//        t[3] = q / v;
//        t[4] = t[3] + t[0];
//        t[5] = t[3] + t[1];
//        t[6] = t[3] + t[0] + t[1];
//        if (t[0] >= t[1] || t[2] >= t[3])
//        {
//            ZLOGE("Joint Space S-Curve Planning Failed, Parameter invalid, please ensure: a/J < v/a && a/J+v/a < q/v!");
//            return E_PLAN_PARA_INVALID;
//        }
//        else
//        {
//            ZLOGD("Joint Space S-Curve Planning Success, whole trajectory will move within %f seconds.", t[6]);
//        }
//
//        int N = static_cast<int>(std::ceil(t[6] * sample_rate) + 1);
//        planner.traj.resize(N);
//        for (int i = 0; i < N; i++)
//        {
//            double time = 1.0 / sample_rate * i;
//            if (time <= t[0])
//            {
//                planner.traj[i] = K[0][0] * std::pow(time, 3) + C[0];
//            }
//            else if (time > t[0] && time <= t[1])
//            {
//                planner.traj[i] = K[1][0] * std::pow(time, 2) + K[1][1] * time + C[1];
//            }
//            else if (time > t[1] && time <= t[2])
//            {
//                planner.traj[i] = K[2][0] * std::pow(time, 3) + K[2][1] * std::pow(time, 2) + K[2][2] * time + C[2];
//            }
//            else if (time > t[2] && time <= t[3])
//            {
//                planner.traj[i] = K[3][0] * time + C[3];
//            }
//            else if (time > t[3] && time <= t[4])
//            {
//                planner.traj[i] = K[4][0] * std::pow(time, 3) + K[4][1] * std::pow(time, 2) + K[4][2] * time + C[4];
//            }
//            else if (time > t[4] && time <= t[5])
//            {
//                planner.traj[i] = K[5][0] * std::pow(time, 2) + K[5][1] * time + C[5];
//            }
//            else if (time > t[5] && time <= t[6])
//            {
//                planner.traj[i] = K[6][0] * std::pow(time, 3) + K[6][1] * std::pow(time, 2) + K[6][2] * time + C[6];
//            }
//            else
//            {
//                planner.traj[i] = qe;
//            }
//        }
//
//        if (planner.pos_end < planner.pos_start)
//        {
//            std::reverse(planner.traj.begin(), planner.traj.end());
//        }
//
//        return E_PLAN_NO_ERROR;
//    }
} // namespace ZORAL